#include "MPC_EW2D_PatternedW_b.h"

// test_MPC_EW2D_equations.out T W A
int main(int argc, char** argv){
  double x[3];
  double epsilon[3];
  double value;
  double grad[3];
  double LatticeSize = 100;

  x[0] = atof(argv[1]);
  x[1] = atof(argv[2]);
  x[2] = atof(argv[3]);
  epsilon[0]= 1e-10;
  epsilon[1]= 1e-10;
  epsilon[2]= 1e-10;

  //  cout << "c2(T,W)\n";
  MathFunction* c2 = new eq_C2();
  c2->parameterize(&LatticeSize);
  value = c2->value(x);
  c2->gradient(x,grad);
  //  cout << "x = [" << x[0] << ", " << x[1] << "]\n";
  //cout << "c2 = " << value << "\n";
  //cout << "grad c2 = [" << grad[0] << ", " << grad[1] << "]\n";
  //  perturb_MathFunction(c2,x,epsilon);
  
  //  cout << "\nsigma2(T,W)\n";
  MathFunction* sigma2 = new eq_sigma2();
  //  perturb_MathFunction(sigma2,x,epsilon);

  //  cout << "\ncov_z_p(T,W)\n";
  MathFunction* cov_z_p = new eq_cov_z_p();
  cov_z_p->parameterize(0,2.0);
  cov_z_p->parameterize(1,5.0);
  cov_z_p->parameterize(2,0.0);
  //  perturb_MathFunction(cov_z_p,x,epsilon);

  /*  cout << "\ncov_z_p2(T,W)\n";
  double y[2];
  double epsilon_y[2];
  MathFunction* cov_z_p2 = new eq_cov_z_p2();
  y[0] = c2->value(x);
  y[1] = sigma2->value(x);
  epsilon_y[0]= 1e-15;
  epsilon_y[1]= 1e-15;
  perturb_MathFunction(cov_z_p2,y,epsilon_y);*/

  cout << "z2_p(T,W,A)\n";
  MathFunction* z2_p = new eq_z2_p();
  int p,m,n;
  /*
  for(m=0;m<=50;m++){
    for(n=0;n<=50;n++){
      if(m!=0 || n!=0){
	for(p=0;p<4;p++){
	  z2_p->parameterize(0,p);
	  z2_p->parameterize(1,m);
	  z2_p->parameterize(2,n);
	  cout << "p=" << p << ", m=" << m << ", n=" << n << "\n";
	  if(perturb_MathFunction(z2_p,x,epsilon)!=0){
	    cout << "ERROR @ p=" << p << ", m=" << m << ", n=" << n << "\n";
	  }
	}
      }
    }
  }
  */

  cout << "z2_p(T,W,A), (p,m,n) = (2,5,0)\n";
  z2_p->parameterize(0,2.0);
  z2_p->parameterize(1,5.0);
  z2_p->parameterize(2,0.0);
  double x1[3];
  double epsilon1[3];
  x1[0] = x[0];
  x1[1] = x[1];
  x1[2] = x[2];
  //  epsilon1[0] = 0.00001*x1[0];
  epsilon1[0] = 5;
  epsilon1[1] = 1e-13;
  epsilon1[2] = 1e-13;
  perturb_MathFunction(z2_p,x1,epsilon1);
  // 12.2363, -45.0286, 0

  //  cout << "z2_p(T,W,A), (p,m,n) = (1,5,0)\n";
  //  z2_p->parameterize(0,1.0);
  //  z2_p->parameterize(1,5.0);
  //  z2_p->parameterize(2,0.0);
  //  perturb_MathFunction(z2_p,x1,epsilon1);


  double* mean_z_m[4];
  double* cov_z_m[4];
  for(int i=0;i<4;i++){
    mean_z_m[i] = new double[2601];
    cov_z_m[i] = new double[2601];
    for(int j=0;j<2601;j++){
      mean_z_m[i][j] = 0.0;
      cov_z_m[i][j] = 0.0;
    }
  }
  cout << "\n r2_f(T,W,A)\n";
  MathFunction* r2_f = new eq_r2_f();
  r2_f->parameterize(0,cov_z_m[0]);
  r2_f->parameterize(1,mean_z_m[0]);
  r2_f->parameterize(2,cov_z_m[1]);
  r2_f->parameterize(3,mean_z_m[1]);
  r2_f->parameterize(4,cov_z_m[2]);
  r2_f->parameterize(5,mean_z_m[2]);
  r2_f->parameterize(6,cov_z_m[3]);
  r2_f->parameterize(7,mean_z_m[3]);
  perturb_MathFunction(r2_f,x1,epsilon1);
  double grad_r2_f[3];
  r2_f->gradient(x,grad_r2_f);

  //  cout << "\n m2_f(T,W,A)\n";
  MathFunction* m2_f = new eq_m2_f();
  m2_f->parameterize(0,cov_z_m[0]);
  m2_f->parameterize(1,mean_z_m[0]);
  m2_f->parameterize(2,cov_z_m[1]);
  m2_f->parameterize(3,mean_z_m[1]);
  m2_f->parameterize(4,cov_z_m[2]);
  m2_f->parameterize(5,mean_z_m[2]);
  m2_f->parameterize(6,cov_z_m[3]);
  m2_f->parameterize(7,mean_z_m[3]);
  //  perturb_MathFunction(m2_f,x,epsilon1);
  double grad_m2_f[3];
  m2_f->gradient(x,grad_m2_f);

  //  cout << "\n f(T,W,A)\n";
  MathFunction* cost_fun = new eq_f();
  cost_fun->parameterize(0,cov_z_m[0]);
  cost_fun->parameterize(1,mean_z_m[0]);
  cost_fun->parameterize(2,cov_z_m[1]);
  cost_fun->parameterize(3,mean_z_m[1]);
  cost_fun->parameterize(4,cov_z_m[2]);
  cost_fun->parameterize(5,mean_z_m[2]);
  cost_fun->parameterize(6,cov_z_m[3]);
  cost_fun->parameterize(7,mean_z_m[3]);  
  //  perturb_MathFunction(cost_fun,x,epsilon1);

  double T;
  double W = x[1];
  double A = x[2];

  ofstream f_log("test_MathFunction_function.log");
  for(T = 599;T<=601;T=T+1e-3){
    x[0] = T;
    x[1] = W;
    x[2] = A;
    f_log << scientific;
    f_log << setw(15) << setprecision(6) << T 
	  << setw(15) << setprecision(6) << W 
	  << setw(15) << setprecision(6) << A 
      //	  << setw(15) << cost_fun->value(x) 
      //  << setw(15) << r2_f->value(x)
	  << setw(20) << setprecision(9)<< z2_p->value(x)
	  << "\n";
  }
  
  delete c2;
  delete sigma2;
  delete cov_z_p;
  //  delete cov_z_p2;
  delete z2_p;
  delete r2_f;
  delete m2_f;
  delete cost_fun;
}
